/*
 * To change this license header, choose License Headers in Project Properties.
 * To change this template file, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.wpi.first.wpilibj.templates.subsystems;

import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.AnalogChannel;
import edu.wpi.first.wpilibj.AnalogModule;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.templates.RobotMap;
import edu.wpi.first.wpilibj.templates.commands.GetDistance;

/**
 *
 * @author Robotics
 */
public class RangeFinder extends Subsystem {
    // Put methods for controlling this subsystem
    // here. Call these from Commands
    private AnalogChannel RangeFinder;
    private AnalogModule Ping;
    
    public void initDefaultCommand() {
        // Set the default command for a subsystem here.
        //setDefaultCommand(new MySpecialCommand());
        setDefaultCommand(new GetDistance());
    }

    public RangeFinder(){
        RangeFinder = new AnalogChannel(RobotMap.RANGEFINDER_AC);
        Ping = RangeFinder.getModule();
    }

    public double getRange() {
       /*double Volt;
       Volt= theRangeFinder.getAverageVoltage();
        double Distance = 0;
        
        Distance = Volt / (5.09 / 512.0);
        SmartDashboard.putNumber("Distance", Distance);
        return Distance;*/
        double Voltage = 0, Distance = 0;
        
            Voltage = (5.09 / 1024.0);
            Distance = (Ping.getAverageVoltage(RobotMap.RANGEFINDER_AC)) / Voltage;
            Distance /= 2.54;//cm to inch
            //Distance = (Ping.getAverageVoltage(RobotMap.RANGEFINDER_AC)) / Voltage;//gives disance in cm
            //Distance *= .0328084;//cm to feet
        
        
        return Distance;
    }
}
